#include <quaternion.hpp>

Quaternion::Quaternion()
	: m_Q { 0, 0, 0, 0 }
{
}

Quaternion::Quaternion(const Eigen::Vector4d& v)
	: m_Q(v(0), v(1), v(2), v(3))
{
}

Quaternion::Quaternion(double q0, double q1, double q2, double q3)
	: m_Q(q0, q1, q2, q3)
{
}

Quaternion::Quaternion(const Quaternion& other)
{
	m_Q = other.m_Q;
}

Quaternion::~Quaternion()
{
}

void Quaternion::Normalize(void)
{
	double norm = sqrt(m_Q(0) * m_Q(0) + m_Q(1) * m_Q(1) + m_Q(2) * m_Q(2) + m_Q(3) * m_Q(3));
	m_Q(0) /= norm;
	m_Q(1) /= norm;
	m_Q(2) /= norm;
	m_Q(3) /= norm;
}

void Quaternion::Unit(void)
{
	m_Q(0) = 1;
	m_Q(1) = 0;
	m_Q(2) = 0;
	m_Q(3) = 0;
}

Quaternion Quaternion::Normalized(void)
{
	Quaternion q(*this);
	double norm = sqrt(q(0) * q(0) + q(1) * q(1) + q(2) * q(2) + q(3) * q(3));
	q(0) /= norm;
	q(1) /= norm;
	q(2) /= norm;
	q(3) /= norm;
	return q;
}

Eigen::Vector4d Quaternion::Derivative1(Eigen::Vector3d w)
{
	Eigen::Vector4d r;
	Eigen::Vector4d v;

	v(0) = 0;
	v(1) = w(0);
	v(2) = w(1);
	v(3) = w(2);

	r(0) = m_Q(0) * v(0) - m_Q(1) * v(1) - m_Q(2) * v(2) - m_Q(3) * v(3);
	r(1) = m_Q(0) * v(1) + m_Q(1) * v(0) + m_Q(2) * v(3) - m_Q(3) * v(2);
	r(2) = m_Q(0) * v(2) - m_Q(1) * v(3) + m_Q(2) * v(0) + m_Q(3) * v(1);
	r(3) = m_Q(0) * v(3) + m_Q(1) * v(2) - m_Q(2) * v(1) + m_Q(3) * v(0);

	return r;
}

Eigen::Vector3d Quaternion::ToToEulerAngles(void)
{
	Eigen::Vector3d angles;

	// 计算滚转角（Roll）
	angles(0) = atan2(2 * (m_Q(0) * m_Q(1) + m_Q(2) * m_Q(3)), 1 - 2 * (m_Q(1) * m_Q(1) + m_Q(2) * m_Q(2)));

	// 计算俯仰角（Pitch）
	double sinp = 2 * (m_Q(0) * m_Q(2) - m_Q(3) * m_Q(1));
	if (fabs(sinp) >= 1)
	{
		// 避免极限情况下的除零错误
		angles(1) = copysign(M_PI / 2, sinp);
	}
	else
	{
		angles(1) = asin(sinp);
	}

	// 计算偏航角（Yaw）
	angles(2) = atan2(2 * (m_Q(0) * m_Q(3) + m_Q(1) * m_Q(2)), 1 - 2 * (m_Q(2) * m_Q(2) + m_Q(3) * m_Q(3)));

	return angles;
}

double& Quaternion::operator()(int index)
{
	return m_Q(index);
}

Quaternion Quaternion::operator+(const Quaternion& other)
{
	return Quaternion(m_Q + other.m_Q);
}

Quaternion Quaternion::operator+=(const Quaternion& other)
{
	m_Q += other.m_Q;
	return *this;
}

Quaternion Quaternion::operator+(const Eigen::Vector4d& other)
{
	Quaternion o(other);
	return Quaternion(m_Q + o.m_Q);
}

Quaternion Quaternion::operator+=(const Eigen::Vector4d& other)
{
	Quaternion o(other);
	m_Q += o.m_Q;
	return *this;
}

Quaternion Quaternion::operator*(const Quaternion& other)
{
	Quaternion q(*this);
	q(0) = m_Q(0) * other.m_Q(0) - m_Q(1) * other.m_Q(1) - m_Q(2) * other.m_Q(2) - m_Q(3) * other.m_Q(3);
	q(1) = m_Q(0) * other.m_Q(1) + m_Q(1) * other.m_Q(0) + m_Q(2) * other.m_Q(3) - m_Q(3) * other.m_Q(2);
	q(2) = m_Q(0) * other.m_Q(2) - m_Q(1) * other.m_Q(3) + m_Q(2) * other.m_Q(0) + m_Q(3) * other.m_Q(1);
	q(3) = m_Q(0) * other.m_Q(3) + m_Q(1) * other.m_Q(2) - m_Q(2) * other.m_Q(1) + m_Q(3) * other.m_Q(0);
	return q;
}

Quaternion Quaternion::operator*=(const Quaternion& other)
{
	Quaternion q1(*this);
	m_Q(0) = q1.m_Q(0) * other.m_Q(0) - q1.m_Q(1) * other.m_Q(1) - q1.m_Q(2) * other.m_Q(2) - q1.m_Q(3) * other.m_Q(3);
	m_Q(1) = q1.m_Q(0) * other.m_Q(1) + q1.m_Q(1) * other.m_Q(0) + q1.m_Q(2) * other.m_Q(3) - q1.m_Q(3) * other.m_Q(2);
	m_Q(2) = q1.m_Q(0) * other.m_Q(2) - q1.m_Q(1) * other.m_Q(3) + q1.m_Q(2) * other.m_Q(0) + q1.m_Q(3) * other.m_Q(1);
	m_Q(3) = q1.m_Q(0) * other.m_Q(3) + q1.m_Q(1) * other.m_Q(2) - q1.m_Q(2) * other.m_Q(1) + q1.m_Q(3) * other.m_Q(0);
	return *this;
}

Quaternion Quaternion::operator*(const double& other)
{
	return Quaternion(m_Q * other);
}

Quaternion Quaternion::operator*=(const double& other)
{
	m_Q *= other;
	return *this;
}
